#pragma once

#include <ddblk/apds/combine_int.h>
#include <dbg.h>

#include <unix/coreaff.h>

typedef struct _ffwdnode{
	struct{
		APDSFUTURE_DECL_NOFLAG
		word_t flag;
	} CACHE_ALIGNED;
} _ffwdnode_t;

#define FFWD_MAX_SERVER		(1)

#define FFWD_REQS_PERGROUPS	(16)	
#define FFWD_FINISHED		(0x1)


#define FFWD_MAX_GROUPS 	(APDS_MAX_THREADS/FFWD_REQS_PERGROUPS)
#define FFWD_GROUPID(tid)	(tid / (FFWD_REQS_PERGROUPS - 1))
#define FFWD_GROUP_TID(tid)	(tid % (FFWD_REQS_PERGROUPS - 1))

typedef struct _ffwdserver {
	/* FFWD_REQS_PERGROUPS - 1 是flag */
	word_t server_responses[FFWD_MAX_GROUPS][FFWD_REQS_PERGROUPS];
	_ffwdnode_t node[FFWD_MAX_GROUPS*(FFWD_REQS_PERGROUPS - 1)];
	lkword_t state;
	cid_t cid;
} PAGE_ALIGNED _ffwdserver_t;

typedef struct _ffwd {
	lkword_t server_id;
} _ffwd_t;

DECL_COMBINE(ffwd)

/* 使用静态服务器 */
SAI _ffwdserver_t *ffwd_server_get(lkword_t server_id){
	static _ffwdserver_t ffwd_servers[FFWD_MAX_SERVER];
	return &ffwd_servers[server_id];
}

SAI void ffwd_client_set(){
	static TLS_INITIAL bool_t coreset_exclusive;
	if (!coreset_exclusive){
		/* 只排除服核 */
		// cpu_set_t __mycpuset; 
		// cid_t server_core = (HDEF_LITTLECORE_OFFST + HDEF_NATIVE_CPU_NR - 1) % HDEF_NATIVE_CPU_NR;
		// if (sched_getaffinity(pthread_self(), sizeof(__mycpuset), &__mycpuset) == 0) {
		// 	/* 如果在本核上，则迁移到下一个核心 */
		// 	if (__CPU_ISSET_S(server_core, sizeof(cpu_set_t), &__mycpuset)){
		// 		__CPU_CLR_S(server_core, sizeof(cpu_set_t), &__mycpuset);
		// 		__CPU_SET_S((server_core + 1) % HDEF_NATIVE_CPU_NR, sizeof(cpu_set_t), &__mycpuset);
		// 	}
		// 	/* 提交 */
		// 	while (0 != try_commit_aff(&__mycpuset)){ 
		// 		/* printf("failed to exclude core to %s\n", #__VA_ARGS__); */ 
		// 	}
		// }

		/* 排除服务核，允许在任何核心上调度 */
		cid_t server_core = (HDEF_LITTLECORE_OFFST + HDEF_NATIVE_CPU_NR - 1) % HDEF_NATIVE_CPU_NR;
		cpu_set_t __mycpuset; __memset(&__mycpuset, 0xff, sizeof(cpu_set_t));
		__CPU_CLR_S(server_core, sizeof(cpu_set_t), &__mycpuset);
		while (0 != try_commit_aff(&__mycpuset)){
			// dbg_INFO("set exclusive failed\n");
		}
		coreset_exclusive = 1;
	}
}

SAI void ffwd_server_init(_ffwdserver_t *server){
	__memset(server, 0, sizeof(_ffwdserver_t));
}

SAI void __ffwd_incr_prio(pthread_t *pth){
	pthread_attr_t attr;
	pthread_getattr_np(*pth, &attr);
	int policy;
	pthread_attr_getschedpolicy(&attr, &policy);
	int ret = pthread_setschedprio(*pth, sched_get_priority_max(policy));
	if (ret != 0){
		INFO("[err: %d]ffwd server incr prio to max failed\n", ret);
	}
	pthread_attr_destroy(&attr);
}

SAI void* ffwd_server_func(void *arg){
	/* 创建时由外部传入 */
	_ffwdserver_t *s = (_ffwdserver_t *)arg;
	/* 绑核 */
	BIND_CORE(s->cid);
	/* 最后一个是bits */
	word_t local_response[FFWD_MAX_GROUPS][FFWD_REQS_PERGROUPS] = {0};
	word_t *cached_response;
	INFO("ffwd server func running...\n");
	// INFO("ffwd server func running...\n");


	while (s->state != FFWD_FINISHED){
		extern cid_t __max_threads;
		tid_t max_tid = atm_ld(&__max_threads);
		word_t helped_group_nrs = FFWD_GROUPID(max_tid) + 1; 
		
		_ffwdnode_t *cur;
		tid_t cur_group_tid = 0;
		tid_t cur_tid = 0;
		word_t help_cnt = 0;
		word_t ori_flag;
		
		#pragma unroll FFWD_MAX_GROUPS
		for (word_t gidx = 0; gidx < helped_group_nrs; gidx++){
			/* 每组帮助 */
			/* 获得上一次的flag */
			cached_response = &local_response[gidx][0];
			ori_flag = cached_response[FFWD_REQS_PERGROUPS - 1];

			#pragma unroll FFWD_REQS_PERGROUPS
			for (tid_t tidx = 0; tidx < FFWD_REQS_PERGROUPS - 1; tidx++){
				/* 组内帮助 */
				cur_tid = cur_group_tid + tidx;
				cur = &s->node[cur_tid];
				/* 相反说明需要帮助 */
				if ((atm_ld(&cur->flag) ^ ori_flag) & BIT(tidx))
				{
					INFO("complete\n");
					cached_response[tidx] = apds_sync_single(cur->func, &cur->args);
					/* 翻转位 */
					cached_response[FFWD_REQS_PERGROUPS - 1] ^= BIT(tidx);
					help_cnt++;
				}
				/* 如果不拷贝原有结果 */
				/* else{
					cached_response[tidx] = s->server_responses[gidx][tidx];
				} */
			}
			/* 增加 */
			cur_group_tid +=  FFWD_REQS_PERGROUPS - 1;
			
			if (ori_flag ^ cached_response[FFWD_REQS_PERGROUPS - 1]) {
				/* 写回 */
				INFO("write result back %08lx\n", cached_response[FFWD_REQS_PERGROUPS - 1]);
			#if !defined(HDEF_WEAK_MEMORY_ORDER)  && !defined(__clang__)
				/* FIXME: clang O3编译这个会出错?? 修复这个之后请去掉 defined(__clang__) */
				/* 利用sse指令加速拷贝过程 */
				__memcpy(&s->server_responses[gidx][0], cached_response, FFWD_REQS_PERGROUPS * sizeof(word_t));
			#else
				#pragma unroll FFWD_REQS_PERGROUPS
				for(tid_t tidx = 0; tidx < FFWD_REQS_PERGROUPS - 1; tidx++){
					atm_st(&s->server_responses[gidx][tidx], cached_response[tidx]);
				}
				/* 结果写回之前确保有一次memory barrier */
				atm_strel(&s->server_responses[gidx][FFWD_REQS_PERGROUPS - 1], cached_response[FFWD_REQS_PERGROUPS - 1]);
			#endif
			}
		}
	}
	/* 返回 */
	return NULL;	
}

SAI void __ffwd_init(_ffwd_t *s)
{
	static hword_t ffwd_locks = 0;
	hword_t cur_lockid = faa(&ffwd_locks, 1);
	
	/* 分配一个 server */
	s->server_id = cur_lockid % FFWD_MAX_SERVER;
	pthread_t pth;
	if (cur_lockid == 0) {
		/* 第一个锁负责创建ffwd服务器线程 */
		for (word_t server_idx = 0; server_idx < FFWD_MAX_SERVER; server_idx++){
			_ffwdserver_t *s = ffwd_server_get(server_idx);
			ffwd_server_init(s);
			/* 优先绑大核 */
			s->cid = (HDEF_LITTLECORE_OFFST + HDEF_NATIVE_CPU_NR - 1 - server_idx) % HDEF_NATIVE_CPU_NR;
			pthread_create_wrapper(&pth, NULL, ffwd_server_func, s);
			/* 提升优先级 */
			__ffwd_incr_prio(&pth);
		}
	}
}
SAI void *__ffwd_destruct(_ffwd_t *s){
	/* TODO: 负载均衡 */
	return (void *)s;
}

SAI _ffwdnode_t *__ffwd_fetch(_ffwd_t *s, apdsfunc_t *pfunc, apdsarg_t *args, apdstinfo_t *tinfo){
	tid_t tid = tinfo->tid;
	_ffwdserver_t *serv = ffwd_server_get(s->server_id);
	_ffwdnode_t *node = &serv->node[tid];
	word_t local_flag = node->flag ^ BIT(FFWD_GROUP_TID(tid)); 
	/* 参数入队 */
	apds_movargs(&node->args, args);
	/* 通知 */
	node->func = pfunc;
	/* 写入flag */
	atm_strel(&node->flag, local_flag);

	return node;
}

SAI apdsret_t __ffwd_wait(_ffwd_t *s, _ffwdnode_t *node, INFUN(cmbn_wait, waitpolicy), apdstinfo_t *tinfo){
	PARAM_USED(waitpolicy);
	waitcnt_t cnt = -1;
	lkword_t locked = 1;
	lkword_t unlocked = 0;
	lkword_t lock;
	word_t pending;
	_ffwdserver_t *serv = ffwd_server_get(s->server_id);
	word_t local_flag = node->flag;
	tid_t tid = tinfo->tid;
	tid_t ingroup_offst = FFWD_GROUP_TID(tid);
	word_t mask = BIT(ingroup_offst);
	word_t group_id = FFWD_GROUPID(tid);
	
	/* 设置一些参数 */
	ffwd_client_set();

	INFO("expect result %08lx, mask %08lx\n", local_flag, mask);
	while((atm_ldacq(&serv->server_responses[group_id][FFWD_REQS_PERGROUPS - 1]) ^ local_flag) & mask){
		/* 使用自旋等待 */
		// PAUSE();
		/* 使用NOP等待(最大并发) */
		NOP();
		INFO("read %08lx\n", atm_ldacq(&serv->server_responses[group_id][FFWD_REQS_PERGROUPS - 1]));
	}
	INFO("ok\n");
	/* 获取结果*/
	return atm_ld(&serv->server_responses[group_id][ingroup_offst]);
}

PARAM_NONULL(1) SAI _ffwd_t *_ffwd_init(void *ptr, size_t size, INFUN(cmbn_node, nodemgr))
{
	PARAM_USED(nodemgr);
	if (size < sizeof(_ffwd_t)) return NULL;
	_ffwd_t *f = (_ffwd_t *)ptr;
	__ffwd_init(f);
	return f;
}
PARAM_NONULL(1) SAI void *_ffwd_destruct(_ffwd_t *f, INFUN(cmbn_node, nodemgr)){
	PARAM_USED(nodemgr);
	void *rptr = __ffwd_destruct(f);
	return rptr;
}
PARAM_NONULL(1, 2, 3) SAI _ffwdnode_t *_ffwd_fetch(_ffwd_t *f, apdsfunc_t *pfunc, apdsarg_t *args, apdstinfo_t *tinfo, INFUN(cmbn_node, nodemgr)){
	PARAM_USED(nodemgr);
	return __ffwd_fetch(f, pfunc, args, tinfo);
}
PARAM_NONULL(1, 2) SAI apdsret_t _ffwd_wait(_ffwd_t *f, _ffwdnode_t *node, apdstinfo_t *tinfo, INFUN(cmbn_wait, waitpolicy), INFUN(cmbn_node, nodemgr)){
	PARAM_USED(nodemgr);
	apdsret_t ret = __ffwd_wait(f, node, waitpolicy, tinfo);
	return ret;
}
